// Standard includes 
#include <stdlib.h>
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <vector>
using std::vector;

// Local includes
#include <lidar/lidar.h>

// Positions to reach
const int posHigh 		= 495;
const int posLow 		= 555;
const int speedHigh 		= 100; // RPM
const int speedLow 		= 10; // RPM

// 2D array to store 3D data
const int posDiff 		= posLow - posHigh;
const int numScans 		= 722;
const int occHeight 		= 40;
const int occWidth 		= 125;
double **map3d;
int **occupancy;

// Initiailize all distance points to 0
void initializeMaps() {
	map3d = (double**)malloc((posDiff+2) * sizeof(double *));
	for(int i=0; i<posDiff+2; i++) {
		map3d[i] = (double*) malloc(numScans * sizeof(double));
	}

	occupancy = (int**)malloc(occHeight * sizeof(int *));
	for(int i=0; i<occHeight; i++) {
		occupancy[i] = (int*) malloc(occWidth * sizeof(int));
	}

	// Initialize the maps
	for(int i=0; i<numScans; i++) {
		for(int j=0; j<posDiff; j++) {
			map3d[j][i] = 4.0;
		}
	}

	for(int i=0; i<occHeight; i++) {
		for(int j=0; j<occWidth; j++) {
			occupancy[i][j] = 0;
		}
	}
}

// Filter the map
void filterMap(double tolerance, int filterSize) {
	// Apply minimum range filter
	for(int i=0; i<posDiff; i++) {
		for(int j=0; j<numScans; j++) {
			double curr = map3d[i][j];
			map3d[i][j] = (curr < 0.2 ? 4.0 : curr);
		}
	}

	// Apply noise filter
	for(int i=0; i<posDiff; i++) {
		for(int j=filterSize; j<numScans-filterSize; j++) {
			double curr = map3d[i][j];
			double currLeft = map3d[i][j-filterSize];
			double currRight = map3d[i][j+filterSize];

			if(curr < 4) {
				if((currLeft > curr-tolerance) && (currRight > curr-tolerance)) {
					map3d[i][j] = currRight;
				} else if((currLeft < curr+tolerance) && (currRight < curr+tolerance)) {
					map3d[i][j] = currRight;
				} else if((currLeft < curr+tolerance) && (currRight > curr-tolerance)) {
					map3d[i][j] = currRight;
				} else if((currLeft > curr-tolerance) && (currRight < curr+tolerance)) {
					map3d[i][j] = currRight;
				}
			}
		}
	}
}

// Use the 3d map to make an occ grid
void constructOccupancyGrid() {
	for(int i=0; i<posDiff; i++) {
		int x = 0;
		for(int j=0; j<numScans; j+=PTS_PER_TEN_CM) {
			for(int k=j; k<j+PTS_PER_TEN_CM; k++) {
				double y = map3d[i][k] * 10;
				if(y < 40) {
					occupancy[(int)y][x] = 2;
				}
			}
			if(++x > occWidth-1) {
				break;
			}
		}
	}
}

// Use cspacing to expand the occupancy grid
void expandOccupancyGrid() {
	for(int i=0; i<occHeight; i++) {
		for(int j=0; j<occWidth; j++) {
			if(occupancy[i][j] == 2) {
				int miny = i - OBSTACLE_GROWTH;
				int maxy = i + OBSTACLE_GROWTH;
				int minx = j - OBSTACLE_GROWTH;
				int maxx = j + OBSTACLE_GROWTH;

				while(miny < 0) miny++;
				while(maxy > occHeight-1) maxy--;
				while(minx < 0) minx++;
				while(maxx > occWidth-1) maxx--;

				for(int y=miny; y<maxy; y++) {
					for(int x=minx; x<maxx; x++) {
						occupancy[y][x] = 1;
					}
				}
			} 
		}
	}
}
